﻿
// 2019/10/30: 首个记录（基于VehicleSampleV3.cs 2019/10/24版本修改）
// 2020/1/10: 增加急动度值；添加构造函数；修正插值函数
// 2020/1/14: 接口完备性
// 2020/6/16: 移除枚举值多余前缀
// 2020/11/16: 实现ToGeneralSample
// 2021/4/6: 基于ASEvaAPI 2.0
// 2022/10/18: 增加SO标签
// 2022/11/3: 使用SetTime复制全部时间信息
// 2023/10/12: 补充部分英文注释

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ASEva;
using SO = System.Reflection.ObfuscationAttribute;

namespace VehicleSampleV4
{
    [SO]
    enum GearPosition
    {
        Unknown = 0, // 未知
        Park = 1, // P档
        Reverse = 2, // R档
        Neutral = 3, // N档
        Drive = 4, // D档
    };

    [SO]
    enum TurnState
    {
        Unknown = 0, // 未知
        None = 1, // 未打灯
        TurnLeft = 2, // 左转向灯
        TurnRight = 3, // 右转向灯
        Emergency = 4, // 双闪
    };

    [SO]
    enum Horn
    {
        Unknown = 0, // 未知
        None = 1, // 不响
        Active = 2, // 响
    };

    [SO]
    enum HeadLight
    {
        Unknown = 0, // 未知
        None = 1, // 未激活
        Near = 2, // 近光
        Far = 3, // 远光
    };

    [SO]
    enum Wiper
    {
        Unknown = 0, // 未知
        None = 1, // 未激活
        Single = 2, // 单次
        Slow1 = 3, // 慢 1
        Slow2 = 4, // 慢 2
        Slow3 = 5, // 慢 3
        Slow4 = 6, // 慢 4
        Slow5 = 7, // 慢 5
        Medium = 8, // 中档
        Fast = 9, // 快档
    };

    class VehicleSample : Sample
    {
        public double Speed { get; set; } // [kph] 本车车速
        public double KilometerAge { get; set; } // [km] 总里程数
        public double? YawRate { get; set; } // [deg/s] 横摆角速度
        public double? Curvature { get; set; } // [1/m] Turning left is positive, while the driving path is / 行驶曲率，左转为正，未来路径: y=(curvature/2)*(x^2)+(curvature)*(frontOverhang+wheelBase)*x
        public double? AccelX { get; set; } // [m/s2] 本车纵向加速度
        public double? AccelY { get; set; } // [m/s2] 本车横向加速度
        public double? JerkX { get; set; } // [m/s3] 本车纵向急动度
        public double? JerkY { get; set; } // [m/s3] 本车横向急动度
        public FloatPoint[] Trajectory { get; set; } // 历史路径
        public FloatPoint[] Contour { get; set; } // The first point and the last point are both (0,0), forming a closed curve / 轮廓描述，以(0,0)开始，以(0,0)结束

        public double VehicleWidth { get; set; } // [m] 车宽
        public double VehicleLength { get; set; } // [m] 车长
        public double VehicleHeight { get; set; } // [m] 车高
        public double SteerAngleRatio { get; set; } // 传动比
        public double WheelBase { get; set; } // [m] 轴距
        public double RearTread { get; set; } // [m] 轮距
        public double FrontOverhang { get; set; } // [m] 前悬

        public double? SteerAngle { get; set; } // [deg] 方向盘转角
        public double? SteerAngleRate { get; set; } // [deg/s] 方向盘转角速度
        public double? SteerTorque { get; set; } // [Nm] 方向盘力矩

        public double? Brake { get; set; } // [%] 刹车位置
        public double? Throttle { get; set; } // [%] 油门位置
        public GearPosition Gear { get; set; } // 档位
        public TurnState Turn { get; set; } // 转向灯
        public Horn Horn { get; set; } // 喇叭
        public HeadLight HeadLight { get; set; } // 大灯
        public Wiper Wiper { get; set; } // 雨刮

        public double? FLSpeed { get; set; } // [kph] Speed of front left wheel / 左前轮轮速
        public double? FRSpeed { get; set; } // [kph] Speed of front right wheel / 右前轮轮速
        public double? RLSpeed { get; set; } // [kph] Speed of rear left wheel / 左后轮轮速
        public double? RRSpeed { get; set; } // [kph] Speed of rear right wheel / 右后轮轮速

        public double? EngineSpeed { get; set; } // [rpm] 引擎转速
        public double? EngineTorque { get; set; } // [Nm] 引擎扭矩

        public double? FrontEndLateralSpeed { get; set; } // [m/s] 前保中心横向速度
        public double? RearEndLateralSpeed { get; set; } // [m/s] 后保中心横向速度

        public VehicleSample()
        {
            VehicleWidth = 1.9;
            VehicleLength = 4.6;
            VehicleHeight = 1.5;
            SteerAngleRatio = 15;
            WheelBase = 2.8;
            RearTread = 1.6;
            FrontOverhang = 0.9;
        }

        public static string Title
        {
            get
            {
                return "Contour point count,Trajectory point count,Speed[KPH],Yaw rate[°/s],Curvature[1/m],Acceleration-x[m/s2],Acceleration-y[m/s2],Vehicle width[m],Vehicle length[m],Vehicle height[m],Steer angle ratio,Wheel base[m],Rear tread[m],Front overhang[m],Steer angle[°],Steer angle rate[°/s],Brake position[%],Throttle position[%],Gear position,Turn,Wheel speed (fl)[KPH],Wheel speed (fr)[KPH],Wheel speed (rl)[KPH],Wheel speed (rr)[KPH],Engine speed[RPM],Engine torque[Nm],Horn,Head light,Wiper,Total distance[KM],Front-end lateral speed[m/s],Rear-end lateral speed[m/s],Steer angle torque[Nm],Jerk-x[m/s3],Jerk-y[m/s3],Reserved1,Reserved2,Reserved3,Reserved4,Reserved5,Contour points...";
            }
        }

        public static string Protocol
        {
            get
            {
                return "vehicle-sample-v4";
            }
        }

        public static string[] Protocols
        {
            get
            {
                return new String[] { "vehicle-sample-v3", "vehicle-sample-v4" }; 
            }
        }

        public override string GetGeneralSampleProtocol()
        {
            return Protocol;
        }

        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }

        public override GeneralSample ToGeneralSample()
        {
            var sample = new GeneralSample();
            sample.SetTime(this);
            sample.Protocol = Protocol;

            var contPointCount = Contour == null ? 0 : Contour.Length;
            var trajPointCount = Trajectory == null ? 0 : Trajectory.Length;
            var data = new GeneralSampleValue[40 + (contPointCount + trajPointCount) * 2];

            data[0] = new GeneralSampleValue((double)contPointCount);
            data[1] = new GeneralSampleValue((double)trajPointCount);
            data[2] = new GeneralSampleValue(Speed);
            if (YawRate != null ) data[3] = new GeneralSampleValue(YawRate.Value);
            if (Curvature != null) data[4] = new GeneralSampleValue(Curvature.Value);
            if (AccelX != null) data[5] = new GeneralSampleValue(AccelX.Value);
            if (AccelY != null) data[6] = new GeneralSampleValue(AccelY.Value);
            data[7] = new GeneralSampleValue(VehicleWidth);
            data[8] = new GeneralSampleValue(VehicleLength);
            data[9] = new GeneralSampleValue(VehicleHeight);
            data[10] = new GeneralSampleValue(SteerAngleRatio);
            data[11] = new GeneralSampleValue(WheelBase);
            data[12] = new GeneralSampleValue(RearTread);
            data[13] = new GeneralSampleValue(FrontOverhang);
            if (SteerAngle != null) data[14] = new GeneralSampleValue(SteerAngle.Value);
            if (SteerAngleRate != null) data[15] = new GeneralSampleValue(SteerAngleRate.Value);
            if (Brake != null) data[16] = new GeneralSampleValue(Brake.Value);
            if (Throttle != null) data[17] = new GeneralSampleValue(Throttle.Value);
            data[18] = new GeneralSampleValue((double)(int)Gear);
            data[19] = new GeneralSampleValue((double)(int)Turn);
            if (FLSpeed != null) data[20] = new GeneralSampleValue(FLSpeed.Value);
            if (FRSpeed != null) data[21] = new GeneralSampleValue(FRSpeed.Value);
            if (RLSpeed != null) data[22] = new GeneralSampleValue(RLSpeed.Value);
            if (RRSpeed != null) data[23] = new GeneralSampleValue(RRSpeed.Value);
            if (EngineSpeed != null) data[24] = new GeneralSampleValue(EngineSpeed.Value);
            if (EngineTorque != null) data[25] = new GeneralSampleValue(EngineTorque.Value);
            data[26] = new GeneralSampleValue((double)(int)Horn);
            data[27] = new GeneralSampleValue((double)(int)HeadLight);
            data[28] = new GeneralSampleValue((double)(int)Wiper);
            data[29] = new GeneralSampleValue(KilometerAge);
            if (FrontEndLateralSpeed != null) data[30] = new GeneralSampleValue(FrontEndLateralSpeed.Value);
            if (RearEndLateralSpeed != null) data[31] = new GeneralSampleValue(RearEndLateralSpeed.Value);
            if (SteerTorque != null) data[32] = new GeneralSampleValue(SteerTorque.Value);
            if (JerkX != null) data[33] = new GeneralSampleValue(JerkX.Value);
            if (JerkY != null) data[34] = new GeneralSampleValue(JerkY.Value);

            for (int i = 0; i < contPointCount; i++)
            {
                data[40 + 2 * i] = new GeneralSampleValue((double)Contour[i].X);
                data[41 + 2 * i] = new GeneralSampleValue((double)Contour[i].Y);
            }

            var trajectoryBase = 40 + contPointCount * 2;
            for (int i = 0; i < trajPointCount; i++)
            {
                data[trajectoryBase + 2 * i] = new GeneralSampleValue((double)Trajectory[i].X);
                data[trajectoryBase + 2 * i + 1] = new GeneralSampleValue((double)Trajectory[i].Y);
            }

            sample.Values = data.ToList();
            sample.NumberOfSignificants = 40 + contPointCount * 2;

            return sample;
        }

        public override bool FromGeneralSample(GeneralSample sample)
        {
            SetTime(sample);

            if (sample.Protocol == "vehicle-sample-v4")
            {
                if (sample.Values.Count < 40) return false;

                int contourSize = (int)sample.Values[0].number;
                int trajectorySize = (int)sample.Values[1].number;
                int sizeWithoutTraj = 40 + 2 * contourSize;
                int sizeWithTraj = 40 + 2 * (contourSize + trajectorySize);

                if (sample.Values.Count != sizeWithoutTraj && sample.Values.Count != sizeWithTraj) return false;

                Speed = sample.Values[2].IsNumber() ? sample.Values[2].number : 0;
                YawRate = sample.Values[3].ToDouble();
                Curvature = sample.Values[4].ToDouble();
                AccelX = sample.Values[5].ToDouble();
                AccelY = sample.Values[6].ToDouble();
                VehicleWidth = sample.Values[7].IsNumber() ? sample.Values[7].number : 1.9;
                VehicleLength = sample.Values[8].IsNumber() ? sample.Values[8].number : 4.6;
                VehicleHeight = sample.Values[9].IsNumber() ? sample.Values[9].number : 1.5;
                SteerAngleRatio = sample.Values[10].IsNumber() ? sample.Values[10].number : 15;
                WheelBase = sample.Values[11].IsNumber() ? sample.Values[11].number : 2.8;
                RearTread = sample.Values[12].IsNumber() ? sample.Values[12].number : 1.6;
                FrontOverhang = sample.Values[13].IsNumber() ? sample.Values[13].number : 0.9;
                SteerAngle = sample.Values[14].ToDouble();
                SteerAngleRate = sample.Values[15].ToDouble();
                Brake = sample.Values[16].ToDouble();
                Throttle = sample.Values[17].ToDouble();
                Gear = sample.Values[18].IsNumber() ? (GearPosition)(int)sample.Values[18].number : GearPosition.Unknown;
                Turn = sample.Values[19].IsNumber() ? (TurnState)(int)sample.Values[19].number : TurnState.Unknown;
                FLSpeed = sample.Values[20].ToDouble();
                FRSpeed = sample.Values[21].ToDouble();
                RLSpeed = sample.Values[22].ToDouble();
                RRSpeed = sample.Values[23].ToDouble();
                EngineSpeed = sample.Values[24].ToDouble();
                EngineTorque = sample.Values[25].ToDouble();
                Horn = sample.Values[26].IsNumber() ? (Horn)(int)sample.Values[26].number : Horn.Unknown;
                HeadLight = sample.Values[27].IsNumber() ? (HeadLight)(int)sample.Values[27].number : HeadLight.Unknown;
                Wiper = sample.Values[28].IsNumber() ? (Wiper)(int)sample.Values[28].number : Wiper.Unknown;
                KilometerAge = sample.Values[29].IsNumber() ? sample.Values[29].number : 0;
                FrontEndLateralSpeed = sample.Values[30].ToDouble();
                RearEndLateralSpeed = sample.Values[31].ToDouble();
                SteerTorque = sample.Values[32].ToDouble();
                JerkX = sample.Values[33].ToDouble();
                JerkY = sample.Values[34].ToDouble();

                Contour = new FloatPoint[contourSize];
                for (int i = 0; i < contourSize; i++)
                {
                    Contour[i] = new FloatPoint((float)sample.Values[40 + 2 * i].number, (float)sample.Values[41 + 2 * i].number);
                }

                if (sample.Values.Count == sizeWithTraj)
                {
                    Trajectory = new FloatPoint[trajectorySize];
                    for (int i = 0; i < trajectorySize; i++)
                    {
                        Trajectory[i] = new FloatPoint((float)sample.Values[sizeWithoutTraj + 2 * i].number, (float)sample.Values[sizeWithoutTraj + 2 * i + 1].number);
                    }
                }
                else
                {
                    Trajectory = new FloatPoint[0];
                }

                return true;
            }
            else if (sample.Protocol == "vehicle-sample-v3")
            {
                if (sample.Values.Count < 27) return false;

                int trajectorySize = (int)sample.Values[0].number;
                int sizeWithoutTraj = 27;
                int sizeWithTraj = 27 + 2 * trajectorySize;

                if (sample.Values.Count != sizeWithoutTraj && sample.Values.Count != sizeWithTraj) return false;

                Speed = sample.Values[1].IsNumber() ? sample.Values[1].number : 0;
                YawRate = sample.Values[2].ToDouble();
                Curvature = sample.Values[3].ToDouble();
                AccelX = sample.Values[4].ToDouble();
                AccelY = sample.Values[5].ToDouble();
                VehicleWidth = sample.Values[6].IsNumber() ? sample.Values[6].number : 1.9;
                VehicleLength = sample.Values[7].IsNumber() ? sample.Values[7].number : 4.6;
                VehicleHeight = 1.5;
                SteerAngleRatio = sample.Values[8].IsNumber() ? sample.Values[8].number : 15;
                WheelBase = sample.Values[9].IsNumber() ? sample.Values[9].number : 2.8;
                RearTread = sample.Values[10].IsNumber() ? sample.Values[10].number : 1.6;
                FrontOverhang = sample.Values[11].IsNumber() ? sample.Values[11].number : 0.9;
                SteerAngle = sample.Values[12].ToDouble();
                Brake = sample.Values[13].ToDouble();
                Throttle = sample.Values[14].ToDouble();
                Gear = sample.Values[15].IsNumber() ? (GearPosition)(int)sample.Values[15].number : GearPosition.Unknown;
                Turn = sample.Values[16].IsNumber() ? (TurnState)(int)sample.Values[16].number : TurnState.Unknown;
                FLSpeed = sample.Values[17].ToDouble();
                FRSpeed = sample.Values[18].ToDouble();
                RLSpeed = sample.Values[19].ToDouble();
                RRSpeed = sample.Values[20].ToDouble();
                EngineSpeed = sample.Values[21].ToDouble();
                EngineTorque = sample.Values[22].ToDouble();
                Horn = sample.Values[23].IsNumber() ? (Horn)(int)sample.Values[23].number : Horn.Unknown;
                HeadLight = sample.Values[24].IsNumber() ? (HeadLight)(int)sample.Values[24].number : HeadLight.Unknown;
                Wiper = sample.Values[25].IsNumber() ? (Wiper)(int)sample.Values[25].number : Wiper.Unknown;
                KilometerAge = sample.Values[26].IsNumber() ? sample.Values[26].number : 0;

                if (sample.Values.Count == sizeWithTraj)
                {
                    Trajectory = new FloatPoint[trajectorySize];
                    for (int i = 0; i < trajectorySize; i++)
                    {
                        Trajectory[i] = new FloatPoint((float)sample.Values[27 + 2 * i].number, (float)sample.Values[27 + 2 * i + 1].number);
                    }
                }
                else
                {
                    Trajectory = new FloatPoint[0];
                }

                Contour = new FloatPoint[0];

                return true;
            }

            return false;
        }

        public override bool SupportInterpolation()
        {
            return true;
        }

        protected override Sample Interpolate(Sample.SearchResult input)
        {
            var s1 = input.s1 as VehicleSample;
            var s2 = input.s2 as VehicleSample;
            var w1 = input.w1;
            var w2 = input.w2;

            var output = new VehicleSample();

            output.Speed = s1.Speed * w1 + s2.Speed * w2;
            output.KilometerAge = s1.KilometerAge * w1 + s2.KilometerAge * w2;
            output.YawRate = s1.YawRate * w1 + s2.YawRate * w2;
            output.Curvature = s1.Curvature * w1 + s2.Curvature * w2;
            output.AccelX = s1.AccelX * w1 + s2.AccelX * w2;
            output.AccelY = s1.AccelY * w1 + s2.AccelY * w2;
            output.JerkX = s1.JerkX * w1 + s2.JerkX * w2;
            output.JerkY = s1.JerkY * w1 + s2.JerkY * w2;
            output.VehicleWidth = s1.VehicleWidth;
            output.VehicleLength = s1.VehicleLength;
            output.VehicleHeight = s1.VehicleHeight;
            output.SteerAngleRatio = s1.SteerAngleRatio;
            output.WheelBase = s1.WheelBase;
            output.RearTread = s1.RearTread;
            output.FrontOverhang = s1.FrontOverhang;
            output.SteerAngle = s1.SteerAngle * w1 + s2.SteerAngle * w2;
            output.SteerAngleRate = s1.SteerAngleRate * w1 + s2.SteerAngleRate * w2;
            output.SteerTorque = s1.SteerTorque * w1 + s2.SteerTorque * w2;
            output.Brake = s1.Brake * w1 + s2.Brake * w2;
            output.Throttle = s1.Throttle * w1 + s2.Throttle * w2;
            output.Gear = w1 > w2 ? s1.Gear : s2.Gear;
            output.Turn = w1 > w2 ? s1.Turn : s2.Turn;
            output.Horn = w1 > w2 ? s1.Horn : s2.Horn;
            output.HeadLight = w1 > w2 ? s1.HeadLight : s2.HeadLight;
            output.Wiper = w1 > w2 ? s1.Wiper : s2.Wiper;
            output.FLSpeed = s1.FLSpeed * w1 + s2.FLSpeed * w2;
            output.FRSpeed = s1.FRSpeed * w1 + s2.FRSpeed * w2;
            output.RLSpeed = s1.RLSpeed * w1 + s2.RLSpeed * w2;
            output.RRSpeed = s1.RRSpeed * w1 + s2.RRSpeed * w2;
            output.EngineSpeed = s1.EngineSpeed * w1 + s2.EngineSpeed * w2;
            output.EngineTorque = s1.EngineTorque * w1 + s2.EngineTorque * w2;
            output.FrontEndLateralSpeed = s1.FrontEndLateralSpeed * w1 + s2.FrontEndLateralSpeed * w2;
            output.RearEndLateralSpeed = s1.RearEndLateralSpeed * w1 + s2.RearEndLateralSpeed * w2;
            output.Trajectory = w1 > w2 ? s1.Trajectory : s2.Trajectory;
            output.Contour = s1.Contour;

            return output;
        }
    }
}
